module CamReader(
  iClk,
  iPclk,
  iData,
  iX,
  iY,
  iFrame,
  iRstN,
  iCenterX,
  iCenterY,
  //Acos side
  oTrigStart,
  oTrigFunc,
  iTrigBusy,
  iTrigError,
  oTrigData,
  iTrigResult,
  // Results
  oLightNum,
  oLight1,
  oLight2,
  oLight3,
  oReady
);

input iClk;
input iPclk;
input [7:0] iData;
input [9:0] iX, iY;
input [9:0] iCenterX, iCenterY;
input iFrame;
input iRstN;
//External acos
output reg oTrigStart = 0;
output reg[1:0] oTrigFunc = 2'b10;
input  iTrigBusy;
input  iTrigError;
output reg [31:0] oTrigData = 0;
input  [31:0] iTrigResult;
//Result
output reg[5:0] oLightNum = 0;
output reg[95:0] oLight1 = 0, oLight2 = 0, oLight3 = 0;
output reg oReady = 0;

// -------------- Parameters --------------------------------------------------------------
localparam PI2      = 32'h40c90fdb; // 2*Pi
localparam C03      = 31'h3E99999A; // 0.3

//---------------- Internal registers and wires -------------------------------------------
reg[9:0] width = 0, center = 0;
reg[1:0] stateRead = 0;
reg[9:0] readX = 0, readY = 0, readWidth = 0;

// 3 biggest light spot
reg[101:0] light1 = 0, light2 = 0, light3 = 0;
// lights ordered by angle
wire[95:0] orderedLight1, orderedLight2, orderedLight3;
// area of the smallest light
wire[31:0] minArea;
// index of the smallest light
wire[1:0] minIndex;
// index of the light addressed in the memory
wire[1:0] memIndex;

reg start = 0, prevStart = 1, busy = 0;
reg[5:0] state = 0, timer = 0;
reg[9:0] x = 0, y = 0, w = 0;
reg [9:0] centerX = 320, centerY = 240;
reg[31:0] alfa = 0, radius = 0, floatW = 0;
reg[5:0] lightNum = 0;

reg[29:0] fifoIn = 0;
wire[29:0] fifoOut;
wire fifoEmpty, fifoFull;
reg fifoClear = 0;
wire[8:0] fifoUsed;
reg fifoWrite = 0, fifoRead = 0;

reg[5:0] lightAddress = 0;
reg[95:0] lightData = 0;
reg lightWr = 0;
wire[95:0] lightOutA;

reg [31:0] intToFloatA = 0;
wire [31:0] intToFloatResult;

reg [31:0] floatSqrtA = 0;
wire [31:0] floatSqrtResult;

reg[31:0] floatDivA = 0, floatDivB = 0;
wire[31:0] floatDivResult;

reg[31:0] floatAddA = 0, floatAddB = 0;
wire[31:0] floatAddResult;

reg[31:0] floatMultA = 0, floatMultB = 0;
wire[31:0] floatMultResult;

// --------------------- assigns -----------------------------------------------------------

assign {minArea, minIndex} = ((light1[95:64] < light2[95:64]) && (light1[95:64] < light3[95:64])) ? {light1[95:64], 2'd0} :
                              (light2[95:64] < light3[95:64]) ? {light2[95:64], 2'd1} : {light3[95:64], 2'd2};
          
assign memIndex = (lightAddress == light1[101:96] ? 2'd0 : 
                   lightAddress == light2[101:96] ? 2'd1 :
                   lightAddress == light3[101:96] ? 2'd2 : 2'd3 );
                   
assign {orderedLight1, orderedLight2, orderedLight3} = 
                      (lightNum == 'd0) ? 288'd0 : 
                      ((lightNum == 'd1) ? {light1[95:0], 192'd0} : 
                      ((lightNum == 'd2) ? (light1[31:0] < light2[31:0] ? {light1[95:0], light2[95:0], 96'd0} : {light2[95:0], light1[95:0], 96'd0} ) :
                      //lightNum > 2 ?
                                       (((light1[31:0] < light2[31:0]) && (light1[31:0] < light3[31:0])) ? 	{light1[95:0], 
                                          light2[31:0] < light3[31:0] ? {light2[95:0], light3[95:0]} : {light3[95:0], light2[95:0]}} :
                                          light1[31:0] < light2[31:0] ? { light3[95:0], light1[95:0], light2[95:0]} :
                                          light1[31:0] < light3[31:0] ? { light2[95:0], light1[95:0], light3[95:0]} :
                                          light2[31:0] < light3[31:0] ? { light2[95:0], light3[95:0], light1[95:0]} :
                                                                        { light3[95:0], light2[95:0], light1[95:0]})));


// --------------------- always blocks -----------------------------------------------------
always@ (posedge iPclk or negedge iRstN) begin
  if(!iRstN) begin
    fifoRead <= 'b0;
    fifoWrite <= 'b0;
    stateRead <= 'b0;
    width <= 'd0;
  end
  // FIFO write
  else begin
    if((iX == 0) &&(iY == 0)) begin
      fifoIn <= 30'd0;
      fifoWrite <= 'b1;
    end
    else begin
      if(iData > 'd128) begin
        if(width == 'd0) center <= iX;
        //else center <= center + ~width[0];
        width <= width + 1'd1;
        fifoWrite <= 'b0;
      end
      else begin
        width <= 'd0; 
        if(width > 'd2)begin 
          fifoIn <= {center + width[9:1], iY, width};
          fifoWrite <= 'b1;
        end
        else fifoWrite <= 'b0;
      end
    end
  
    // FIFO read
    case(stateRead)
      0: begin  // read fifo if not empty
        if(~fifoEmpty)begin
          fifoRead <= 'b1;
          stateRead <= 'd1;
        end
      end
     
      1: begin  // wait for data 
        fifoRead <= 'b0;
        {readX, readY, readWidth} <= fifoOut;
        start <= 'd1;
        stateRead <= 'd3;
      end
     
      3 : stateRead <= 'd2;
     
      2: begin
        start <= 'b0;
        if(!busy) stateRead <= 'd0;
      end
    endcase
  end 
end


// Process data
always@(posedge iClk or negedge iRstN) begin 
  if(!iRstN) begin
    busy <= 'd0;
    state <= 'd0;
    prevStart <= 1'b1;
    lightNum <= 'd0;
    oReady <= 1'd0;
  end
  else begin
    prevStart <= start;
    if(!busy) begin
      if({prevStart, start} == 'b01) begin
        busy  <= 'd1;
        state <= 'd0;
        timer <= 'd0;
        x <= readX;
        y <= readY;
        w <= readWidth;
        lightWr <= 'd0;
        centerX <= iCenterX;
        centerY <= iCenterY;
      end
    end
    else begin
      case(state)
        0: begin
          if(w == 'd0) begin
            state <= 'd32;
            oReady <= 1'd1;
            oLightNum <= lightNum;
            oLight1 <= orderedLight1;
            oLight2 <= orderedLight2;
            oLight3 <= orderedLight3;
            //fifoClear <= 'b1;
          end
          else begin            
            intToFloatA <= (centerX - x)*(centerX - x) + (centerY - y)*(centerY - y); // R^2
            state <= 'd1;
          end
        end
    
        1: begin   
          if(timer == 'd6)begin
            intToFloatA <= (centerY - y);
            floatSqrtA <= intToFloatResult;   // sqrt(R^2) -> R
            timer <= 'd0;
            state <= 'd3;
          end
          else timer <= timer + 5'b1;
        end
    
        3:begin
          if(timer == 'd16)begin
            floatDivA <= intToFloatResult;  // (cy - y) / R
            floatDivB <= floatSqrtResult;
            radius <= floatSqrtResult;
            intToFloatA <= w;
            timer <= 'd0;
            state <= 'd2;
          end
          else timer <= timer + 5'b1;     
        end
    
        2:begin     
          if(timer == 'd6) begin
            timer <= 'd0;
            state <=  'd6;
            oTrigData <= floatDivResult;  // acos((cy - y) / R) -> alfa
            oTrigStart <= 'b1;
            floatW <= intToFloatResult;
          end
          else timer <= timer + 5'b1;   
        end
    
        6: begin
          state <=  'd4;
          oTrigStart <= 'b0;
        end
    
        4:begin          
          if(iTrigError == 'b1)state <= 'd22;       // ERROR
          else if(iTrigBusy == 'b0) begin
            if(x > centerX) begin
              alfa <= iTrigResult;
              state <= 'd12;
            end
            else begin
              floatAddA <= PI2;
              floatAddB <= {~iTrigResult[31],iTrigResult[30:0]};
              state <=  'd5;       
            end
          end
          lightAddress <= lightNum - 1'd1;
        end

        5:begin
          if(timer == 'd7) begin
            timer <= 'd0;
            state <= 'd13;
            alfa <= floatAddResult;
          end
          else timer <= timer + 1'b1;
        end
    
        13:begin
          state <= 'd12;
        end
    
        12:begin
          if(lightAddress[5] == 1'b0) begin
            floatAddA <= lightOutA[31:0];
            floatAddB <= {~alfa[31], alfa[30:0]};      
            state <= 5'd14;
          end
          else begin
            // create new light
            state <= 'd28;
            lightAddress <= lightNum;
            lightData <= {floatW, radius, alfa};
            lightWr <= 'd1;

            if(lightNum < 3) begin
              case(lightNum)
                0: light1 <= {lightNum, floatW, radius, alfa};
                1: light2 <= {lightNum, floatW, radius, alfa};
                2: light3 <= {lightNum, floatW, radius, alfa};
              endcase
            end
            else if(floatW > minArea) begin
              case(minIndex)
                0: light1 <= {lightNum, floatW, radius, alfa};
                1: light2 <= {lightNum, floatW, radius, alfa};
                2: light3 <= {lightNum, floatW, radius, alfa};
              endcase
            end
          end
        end

        14: begin     
          if(timer == 'd7) begin
            timer <= 'd0;
            if(floatAddResult[30:0] < C03) begin
              // merge ligths
              state <= 'd15;
              floatAddA <= floatW;
              floatAddB <= lightOutA[95:64];
              floatMultA <= alfa;
              floatMultB <= floatW;
             
              //lightData <= {floatW, radius, alfa};
              //lightWr <= 'd1;
            end
            else begin
              lightAddress <= lightAddress - 1'b1;
              state <= 'd12;
            end
          end
          else timer <= timer + 1'b1;
        end
    
        // merge ligths
        // new_angle := (width * alfa + old_area * old_angle) / (width + old_area)
        // new_radius := (width * radius + old_area * old_radius) / (width + old_area)
        // new_area := width + old_area
        15: begin
          case(timer)
            0: begin
              floatMultA <= floatW;
              floatMultB <= radius;
            end
          
            1: begin
              floatMultA <= lightOutA[31: 0]; // light angle
              floatMultB <= lightOutA[95:64]; // ligth area
            end      
            
            2: begin
              floatMultA <= lightOutA[95:64]; // ligth area
              floatMultB <= lightOutA[63:32]; // ligth radius;
            end
            
            4: alfa <= floatMultResult;       // width * alfa
            
            5: radius <= floatMultResult;     // width * radius
            
            6: begin
              floatW <= floatAddResult;       // width + old_area
              floatAddA <= alfa;              // width * alfa
              floatAddB <= floatMultResult;   // old_area * old_alfa       
            end
            
            7: begin
              floatAddA <= radius;            // width * radius
              floatAddB <= floatMultResult;   // old_area * old_radius
            end
            
            13: begin
              floatDivA <= floatAddResult;    // width * alfa + old_area * old_alfa
              floatDivB <= floatW;            // width + old_area
            end
            
            14: begin
              floatDivA <= floatAddResult;    // width * radius + old_area * old_radius
              floatDivB <= floatW;            // width + old_area
            end
            
            19: alfa <= floatDivResult;       // new_angle
            
            20: begin
              lightData <= {floatW, floatDivResult, alfa};
              lightWr <= 'd1;
             
              //update light1..3
              case(memIndex)
                0: light1 <= {lightAddress, floatW, floatDivResult, alfa};
                1: light2 <= {lightAddress, floatW, floatDivResult, alfa};
                2: light3 <= {lightAddress, floatW, floatDivResult, alfa};
                3: begin
                  if(floatW > minArea) begin
                    case(minIndex)
                      0: light1 <= {lightAddress, floatW, floatDivResult, alfa};
                      1: light2 <= {lightAddress, floatW, floatDivResult, alfa};
                      2: light3 <= {lightAddress, floatW, floatDivResult, alfa};
                    endcase
                  end
                end
              endcase
            end
          
            21: begin
              lightWr <= 'd0;
              state <= 'd0;
              busy <= 'd0;
              timer <= 'd0;
            end
          endcase  
          timer <= timer + 1'b1;
        end
        
        // create new light
        28: begin
          lightNum <= lightNum + 1'd1;
          busy <= 'd0;
          lightWr <= 'd0;
          state <= 'd0;
        end
        
        // new frame, reset lightnum, send data
        32: begin
          if(timer == 'd4) begin
            timer <= 'd0;
            busy <= 'd0;
            state <= 'd0;
            lightNum <= 'd0;
            oReady <= 1'd0;
            //fifoClear <= 'b0;
          end
          else timer <= timer + 5'b1;
        end
        
        22: begin
          // TODO: handle acos error
          busy <= 'd0;
        end
        
        default: begin
         // TODO: handle state errors
         busy <= 'd0;
        end
   
      endcase
    end
  end
end


//------------ Submodules ----------------------------

FIFO fifo( 
  .clock(~iPclk),
  .data(fifoIn),
  .rdreq(fifoRead),
  .sclr(fifoClear || ~iRstN),
  .wrreq(fifoWrite),
  .empty(fifoEmpty),
  .full(fifoFull),
  .q(fifoOut),
  .usedw(fifoUsed)
);

//LightRam lights(
// .address_a(lightAddress),
// .address_b((uartAddress - 2) / 12),
// .clock(~iClk),
// .data_a(lightData),
// .data_b(96'd0),
// .wren_a(lightWr),
// .wren_b(1'd0),
// .q_a(lightOutA),
// .q_b(lightOutB)
//);

LightRam1Port lights(
  .address(lightAddress),
  .clock(~iClk),
  .data(lightData),
  .wren(lightWr),
  .q(lightOutA)
);

//6T
IntToSinglePrecFloatConvert i2fp(
 .clock(~iClk),
 .dataa(intToFloatA),
 .result(intToFloatResult)
);

//16T
SinglePrecFloatSqrt sqrt(
  .clock(~iClk),
  .data(floatSqrtA),
  .result(floatSqrtResult)
);

//6T
SinglePrecFloatDiv div(
  .dataa(floatDivA),
  .datab(floatDivB),
  .result(floatDivResult),
  .clock(~iClk)
);

//7T
SinglePrecFloatAdd add(
  .clock(~iClk),
  .dataa(floatAddA),
  .datab(floatAddB),
  .result(floatAddResult)
);


//5T
SinglePrecFloatMult m1(
  .dataa(floatMultA),
  .datab(floatMultB),
  .result(floatMultResult),
  .clock(~iClk)
);

endmodule
